#! /usr/bin/env python

# Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
# You can contact the author at <slynen at ethz dot ch>
# Copyright (C) 2011-2012 Stephan Weiss, ASL, ETH Zurich, Switzerland
# You can contact the author at <stephan dot weiss at ieee dot org>
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

PACKAGE='ai_robot_lcsfl'
import roslib; roslib.load_manifest(PACKAGE)

#from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# @todo Think about levels. Setting most of these to STOP to guarantee atomicity.
# @todo Double check these ranges, defaults

INIT_FILTER     = gen.const("INIT_FILTER",              int_t, 0x00000001, "core_init_filter")
MISC            = gen.const("MISC",                     int_t, 0x00000002, "misc")
SET_HEIGHT      = gen.const("SET_HEIGHT",              int_t, 0x00000004, "core_set_height")


#       Name                 Type      Reconfiguration level                Description   Default   Min   Max
gen.add("core_init_filter",       bool_t,   INIT_FILTER["value"],                    "call filter init using defined scale",                    False)
gen.add("core_set_height",        bool_t,   SET_HEIGHT["value"],                     "call filter init using defined height",                    False)
gen.add("core_height",            double_t, MISC["value"],                           "height in m for init",         1,          0.1,       20)

gen.add("pose_initial_scale",		double_t, 	MISC["value"],               	"value for initial scale",          1,      	0.001,		30)
gen.add("pose_fixed_scale",			bool_t,   	MISC["value"],              	"fix scale",                    	False)
gen.add("pose_fixed_p_ic",			bool_t, 	MISC["value"],					"fix calibration state position", False)
gen.add("pose_fixed_q_ic",			bool_t, 	MISC["value"],					"fix calibration state attitude", False)
gen.add("pose_fixed_p_wv",			bool_t, 	MISC["value"],					"fix world vision position drift", False)
gen.add("pose_fixed_q_wv",			bool_t, 	MISC["value"], 				    "fix world vision attitude drift", False)

gen.add("pose_noise_scale",			double_t, 	MISC["value"],                  "propagation: noise scale (std. dev)",         0.0,		0, 	        10.0)

gen.add("pose_noise_p_wv",			double_t, 	MISC["value"],                  "propagation: noise p_wv (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_q_wv",			double_t, 	MISC["value"],                  "propagation: noise q_wv (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_p_ic",			double_t, 	MISC["value"],                  "propagation: noise p_ic (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_q_ic",			double_t, 	MISC["value"],                  "propagation: noise q_ic (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_meas_p",		double_t, 	MISC["value"],                  "noise for measurement sensor (std. dev)",         0.01,          0,       10)
gen.add("pose_noise_meas_q",		double_t, 	MISC["value"],                  "noise for measurement sensor (std. dev)",         0.01,          0,       10)
gen.add("pose_delay", 				double_t, 	MISC["value"], 					"fix delay in seconds", 0.02, -2.0, 2.0)

gen.add("pose_initial_scale_2",		double_t, 	MISC["value"],               	"value for initial scale",          1,      	0.001,		30)
gen.add("pose_fixed_scale_2",			bool_t,   	MISC["value"],              	"fix scale",                    	False)
gen.add("pose_fixed_p_ic_2",			bool_t, 	MISC["value"],					"fix calibration state position", False)
gen.add("pose_fixed_q_ic_2",			bool_t, 	MISC["value"],					"fix calibration state attitude", False)
gen.add("pose_fixed_p_wv_2",			bool_t, 	MISC["value"],					"fix world vision position drift", False)
gen.add("pose_fixed_q_wv_2",			bool_t, 	MISC["value"], 				    "fix world vision attitude drift", False)

gen.add("pose_noise_scale_2",			double_t, 	MISC["value"],                  "propagation: noise scale (std. dev)",         0.0,		0, 	        10.0)

gen.add("pose_noise_p_wv_2",			double_t, 	MISC["value"],                  "propagation: noise p2_wv (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_q_wv_2",			double_t, 	MISC["value"],                  "propagation: noise q2_wv (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_p_ic_2",			double_t, 	MISC["value"],                  "propagation: noise p2_ic (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_q_ic_2",			double_t, 	MISC["value"],                  "propagation: noise q2_ic (std. dev)",           0.0,        0,          10.0)
gen.add("pose_noise_meas_p_2",		double_t, 	MISC["value"],                  "noise for measurement sensor (std. dev)",         0.01,          0,       10)
gen.add("pose_noise_meas_q_2",		double_t, 	MISC["value"],                  "noise for measurement sensor (std. dev)",         0.01,          0,       10)
gen.add("pose_delay_2", 				double_t, 	MISC["value"], 					"fix delay in seconds", 0.02, -2.0, 2.0)

exit(gen.generate(PACKAGE, "Config", "SinglePoseSensor"))
